#include "insdriverpublisher.h"

insDriverPublisher::insDriverPublisher(ros::NodeHandle &nh)
{
    // 发布器：将坐标和速度信息发布，用来记录waypoint ====> [暂时策略]
    rtk_pub_=nh.advertise<sensor_msgs::NavSatFix>("gps/fix",50);
    gps_pub_=nh.advertise<sensor_msgs::NavSatFix>("gps",50);
    ins_pub_=nh.advertise<nav_msgs::Odometry>("gps/ins",200);
    imu_pub_=nh.advertise<sensor_msgs::Imu>("imu/data",200);
    vel_pub_=nh.advertise<geometry_msgs::TwistStamped>("vel",200);

}

// 发布gps/Fix  (RTK Fix)
void insDriverPublisher::pubGPSFIX(INS_STATUS &ins_)
{
//    if(ins_.fix_type!=NARROW_INT)
//        return;
    sensor_msgs::NavSatFix gpsFix_;
    gpsFix_.header.stamp=ros::Time::now();  // TODO: 用pps
    gpsFix_.header.frame_id="/gps";

    if(ins_.fix_type!=NARROW_INT)
        gpsFix_.status.status=sensor_msgs::NavSatFix::_status_type::STATUS_NO_FIX;
    else if(ins_.fix_type==NARROW_INT)
        gpsFix_.status.status=sensor_msgs::NavSatFix::_status_type::STATUS_FIX;
    gpsFix_.status.service=sensor_msgs::NavSatFix::_status_type::SERVICE_GLONASS;

    gpsFix_.latitude=ins_.latitude;
    gpsFix_.longitude=ins_.longitude;
    gpsFix_.altitude=ins_.altitude;

    gpsFix_.position_covariance_type=sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN;

    gpsFix_.position_covariance[0]=pow(ins_.lat_std,2);
    gpsFix_.position_covariance[4]=pow(ins_.lon_std,2);
    gpsFix_.position_covariance[8]=pow(ins_.alti_std,2);

    gps_pub_.publish(gpsFix_);
    if(ins_.fix_type==NARROW_INT)
        rtk_pub_.publish(gpsFix_);
}

void insDriverPublisher::pubGPSFIX(RTK_STATUS &gps_)
{
//    if(gps_.fix_type!=NARROW_INT_GGA)
//        return;
    sensor_msgs::NavSatFix gpsFix_;
    gpsFix_.header.stamp=ros::Time::now();  // TODO: 用pps
    gpsFix_.header.frame_id="/gps";

    if(gps_.fix_type==PSRDIFF_GGA || gps_.fix_type== NARROW_INT_GGA ||
            gps_.fix_type== NARROW_FLOAT_GGA || gps_.fix_type== WAAS_GGA){
       gpsFix_.status.status=sensor_msgs::NavSatFix::_status_type::STATUS_FIX;
    }else {
       gpsFix_.status.status=sensor_msgs::NavSatFix::_status_type::STATUS_NO_FIX;
    }

    gpsFix_.status.service=sensor_msgs::NavSatFix::_status_type::SERVICE_GLONASS;

    gpsFix_.latitude=gps_.latitude;
    gpsFix_.longitude=gps_.longitude;
    gpsFix_.altitude=gps_.altitude;

    gpsFix_.position_covariance_type=sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;

    gpsFix_.position_covariance[0]=pow(gps_.lat_std,2);
    gpsFix_.position_covariance[4]=pow(gps_.lon_std,2);
    gpsFix_.position_covariance[8]=pow(gps_.alti_std,2);

    gps_pub_.publish(gpsFix_);
    if(gps_.fix_type==NARROW_INT_GGA){
        rtk_pub_.publish(gpsFix_);
    }
}

void insDriverPublisher::pubINS(INS_STATUS &ins_)
{
    nav_msgs::Odometry odom_ins_;
    odom_ins_.header.stamp=ros::Time::now();    // TODO: 用pps
    odom_ins_.header.frame_id="/map";           //
    odom_ins_.child_frame_id="/gps";

    odom_ins_.pose.pose.position.x=ins_.longitude;
    odom_ins_.pose.pose.position.y=ins_.latitude;
    odom_ins_.pose.pose.position.z=ins_.altitude;

    //printf("%f %f %f \n",ins_.roll,ins_.pitch,ins_.yaw);
    geometry_msgs::Quaternion q=tf::createQuaternionMsgFromRollPitchYaw(ins_.roll,ins_.pitch,ins_.yaw);
    odom_ins_.pose.pose.orientation=q;

    odom_ins_.pose.covariance[0]=pow(ins_.lon_std,2);
    odom_ins_.pose.covariance[7]=pow(ins_.lat_std,2);
    odom_ins_.pose.covariance[14]=pow(ins_.alti_std,2);
    odom_ins_.pose.covariance[21]=pow(ins_.roll_std,2);
    odom_ins_.pose.covariance[28]=pow(ins_.pitch_std,2);
    odom_ins_.pose.covariance[35]=pow(ins_.yaw_std,2);

    ins_pub_.publish(odom_ins_);
}

void insDriverPublisher::pubIMU(INS_STATUS &ins_)
{
    sensor_msgs::Imu imu_msg;
    imu_msg.header.stamp=ros::Time::now();    // TODO: 用pps
    imu_msg.header.frame_id="/imu_link";

    //geometry_msgs::Quaternion q=tf::createQuaternionMsgFromRollPitchYaw(ins_.roll,ins_.pitch,ins_.yaw);
    imu_msg.orientation.w=ins_.qw;
    imu_msg.orientation.x=ins_.qx;
    imu_msg.orientation.y=ins_.qy;
    imu_msg.orientation.z=ins_.qz;

    imu_msg.linear_acceleration.x=ins_.acc_x;
    imu_msg.linear_acceleration.y=ins_.acc_y;
    imu_msg.linear_acceleration.z=ins_.acc_z;

    imu_msg.angular_velocity.x=ins_.gyro_x;
    imu_msg.angular_velocity.y=ins_.gyro_y;
    imu_msg.angular_velocity.z=ins_.gyro_z;

    // 待标定
    imu_msg.linear_acceleration_covariance[0]=pow(0.025,2);
    imu_msg.linear_acceleration_covariance[4]=pow(0.025,2);
    imu_msg.linear_acceleration_covariance[8]=pow(0.025,2);

    imu_msg.angular_velocity_covariance[0]=pow(0.01,2);
    imu_msg.angular_velocity_covariance[4]=pow(0.01,2);
    imu_msg.angular_velocity_covariance[8]=pow(0.01,2);

    imu_msg.orientation_covariance[0]=pow(ins_.roll_std,2);
    imu_msg.orientation_covariance[4]=pow(ins_.pitch_std,2);
    imu_msg.orientation_covariance[8]=pow(ins_.yaw_std,2);

    imu_pub_.publish(imu_msg);
}

void insDriverPublisher::pubVEL(INS_STATUS &ins_)
{
    geometry_msgs::TwistStamped vel_msg;
    vel_msg.header.stamp=ros::Time::now();    // TODO: 用pps
    vel_msg.header.frame_id="/imu_link";

    vel_msg.twist.linear.x=ins_.vel_n;
    vel_msg.twist.linear.y=ins_.vel_e;
    vel_msg.twist.linear.z=ins_.vel_d;

    vel_msg.twist.angular.x=ins_.gyro_x;
    vel_msg.twist.angular.y=ins_.gyro_y;
    vel_msg.twist.angular.z=ins_.gyro_z;

    vel_pub_.publish(vel_msg);
}

void insDriverPublisher::publish(INS_STATUS &ins_)
{
    pubGPSFIX(ins_);
    pubINS(ins_);
    pubIMU(ins_);
    pubVEL(ins_);
}

void insDriverPublisher::publish(INS_STATUS &ins_,INS_STATUS &imu_, RTK_STATUS &rtk_)
{
    if(rtk_.updated){
        pubGPSFIX(rtk_);
        rtk_.updated=false;
    }

    if(ins_.updated){
        pubINS(ins_);
        pubVEL(ins_);
        ins_.updated=false;
    }

    if(imu_.updated){
        pubIMU(imu_);
        imu_.updated=false;
    }

}

